%**************************************************************************
%BatBot: Biological inspired Bat roBot.

%Copyright RObotics and Cybernetics Group
%Julian Colorado

% Matlab simulator of bat flight behavior. 
%**************************************************************************

%Kinematics: 
%  1)Perter Corke toolbox: Denavit Hartenberg based on basic rotation matrices are used.
%  2)Featherstone toolbox: quaternions are used.

%Dynamics: Floating base dynamics equations of motion based on Rigid Body Dynamics 

%SMA-based Actuation/sensors: Besides flapping, bats perform morphing motion of their wings, by expanding and 
%contracting the wing membrane that improves aerodynamics and maneuverability. Both actuation and sensing
%capacities are addressed using a phenomenological model of SMA Ni? wires working as artificial arrays of muscles. 
%These muscles extend along the sleketon-bat structure of their wings.The phenomenological model is represented by thermo-mechanical
%variable-state equations. SMA are also used as sensors, where Position/Resistance-Resistance/Force relationships are defined.
%**************************************************************************
clear

%control-points for computing BAT CARPUS and MCP_III trajectories for
%DOwnstroke and Upstroke


% Define Flag_test = 1)Biological Specimen,  2)Bat-robot (Simulation), 3)Bat-robot: V3 (ROBOT)
%**************************************************************************
Flag_test = 3;
%**************************************************************************

%Biological Specimen:
if Flag_test ==1
     Carpus = [0.1 0 0.25; 0.18 0 0.22; 0.28 0 0.1; 0.3 0 -0.02; 0.29 0 -0.08; 0.3 0 -0.07; 0.28 0 0.09; 0.2 0 0.2; 0.1 0 0.25];
     MCP_III = [0.35 0 0.2; 0.4 0 0.33; 0.5 0 0.2; 0.55 0 0.02; 0.48 0 -0.25; 0.45 0 -0.28; 0.25 0 -0.18; 0.22 0 -0.05; 0.35 0 0.15]; 
     time = [1 2 3 4 5 6 7 8 9];
     flag = 3;  %[2 or 3] Reads y-axis from y or z. Cause motion might be within X-Y or X-Z planes (Bio-trjectories are in Z, then flag=3)
end

%Bat-robot (Simulation)
if Flag_test ==2
     Carpus = [5.5 9.5 0; 9.5 6 0;10.5 3.5 0;10.5 1 0;10 4.5 0;10 6 0;7.5 8 0;4.5 10 0]*(0.01/2);
     MCP_III = [18 9 0; 21.5 9 0;23 4 0;23 -2 0;17 -5.5 0;16 -3.5 0;17.5 1 0;16 5 0]*(0.01/2); 
     time = [1 2 3 4 5 6 7 8];
     flag = 2;
end

% % %Bat-robot: V3 (ROBOT)
if Flag_test ==3
    Carpus = [1.8 6.7 0; 4.8 5 0; 6.8 1.9 0;7 0.5 0; 6.5 3 0; 5.5 4.5 0; 4 6 0; 2.5 7 0]*(0.01);
    MCP_III = [13 6.5 0; 15.5 2.8 0; 17.5 -0.5 0; 17.5 -4 0; 16.8 -1.5 0; 16.3 1.4 0; 15 3.5 0; 13 5.5 0]*(0.01); 
    time = [1 2 3 4 5 6 7 8];
    flag = 2;
end

step_time = 0.1;
end_time = 1;

%Kinematics model computation for motphing motion
%**************************************************************************
traj_carpus = BatWings_FULLbeatFB(Carpus,time,step_time,end_time);
traj_MCPIII = BatWings_FULLbeatFB(MCP_III,time,step_time,end_time);

[f, c] = size(traj_carpus);
%freq = 1.3; %(Nominal)Flapping Frequency (wing-beat cycle)
freq = 2.5; %(Overloaded)Flapping Frequency (wing-beat cycle)

%**************************************************************************
[Q_C, Q_MPCIII, dQ_C, dQ_MPCIII,d2Q_C, d2Q_MPCIII, step] = i_kine_bat_wingsFB(traj_carpus,traj_MCPIII,freq,flag,Flag_test);

%Fixing data
[f, c] = size(Q_C.signals.values(:,1));
[f2, c2] = size(Q_MPCIII.signals.values(:,1));
if f > f2
    f = f2;
else
    f2 = f;
end

%Kinematics model computation of flapping motion (2 DoF at shoulder):
%spherical motion of the shoulder
%**************************************************************************
[Q_flap dQ_flap d2Q_flap] = BatWings_flappingFB(f-1,freq);


%Parameters used within the inverse dynamics models

%Humerus+SMA muscles and radius   %thin-rode shape (about CM)
l1=0.055;
l2=0.07;
lc1=l1/2;
lc2=l2/2;
m1=0.005; %humerus+SMA muscles
m2=0.003; %radius+digits
i1=(1/12)*m1*l1^2;  
i2=(1/12)*m2*l2^2;  

%Shoulder/scapula, %cylinder shape (about CM)  
m_s=0.006; % 6 grams
r_s=0.02;
i_s_z=0.5*m_s*r_s^2; 
i_s_x=0.083*m_s*(3*r_s^2+0.05^2);
i_s_y=0.083*m_s*(3*r_s^2+0.05^2);

%base body %cube-shape (servo-case) (about CM)
m_b=0.017;  % 17grams

lw = 0.015; %[m] x-axis
lh = 0.047; %[m] y-axis
ld = 0.015; %[m] z-axis

i_b_x=0.083*m_b*(lh^2+ld^2);        
i_b_y=0.083*m_b*(lw^2+ld^2);        
i_b_z=0.083*m_b*(lh^2+lw^2);    

%MCP-III+digits  %thin-rode shape (about CM)
Ltip =.11; %MCP-III; wrist to wingtip distance
m_dig = 0.001;
i3 = (1/12)*m_dig*Ltip^2; 

%Parameters for PID SMA actuation+dyn+control
rh = 0.005;  %radio del eslab?n: humerus
Ih = i1;    %humerus inertia
c = 0.007;  %Torsional damping coeficient
mh = 0.008; %total wing: humerus+radius+digits
Xh = lc1;  %distance to link CM
g=9.81;


%Kinematics Simulations
%**************************************************************************
%Fixing Q for GUI-plotting issues
[fil, col] = size(Q_flap);

%angular joints

Q(:,1) = -.34*ones(fil,1);  %(FIXED BASE @-20[deg]), joint -0- 
Q(:,2) = Q_flap(:,1);  %Shoulder Flapping motion  joint -1- 
Q(:,3) = Q_C.signals.values(1:f,1)+1.0471; %elbow morphing joint -2-  joitn range (0 -> 30deg = 0 -> 0.4972), real mechanical range (60 -> 90 = 1.0471 -> pi/2 )
Q(:,4) = Q_MPCIII.signals.values(1:f,1)-1.0471; %wrist+digits morphing joint -3-   joitn range (0 -> -31deg = 0 -> 0.4972), real mechanical range (-60 -> -90 = -1.0471 -> -pi/2 )
Q(:,5) = zeros(fil,1);  %(CANCELED) (END EFFECTOR) joint -n-

%Joints Velocities
dQ(:,1) = zeros(fil,1);  %(CANCELED)
dQ(:,2) = dQ_flap(:,1);   
dQ(:,3) = dQ_C.signals.values(1:f,1);
dQ(:,4) = dQ_MPCIII.signals.values(1:f,1);
dQ(:,5) = zeros(fil,1);  %(CANCELED)


%Joints Accelerations
d2Q(:,1) = zeros(fil,1);  %(CANCELED)
d2Q(:,2) = d2Q_flap(:,1);
d2Q(:,3) = d2Q_C.signals.values(1:f,1);
d2Q(:,4) = d2Q_MPCIII.signals.values(1:f,1);
d2Q(:,5) = zeros(fil,1);  %(CANCELED)



%Denavit&Hartenberg parameters
  %      alpha   a    teta        d  rota        m      sx      sy     sz      Ixx      Iyy        Izz    Ixy   Ixz   Iyz fric
%Fixed base  
L0=link([0       0     Q(1,1)     0    0        m_b      0        0     0       i_b_x    i_b_y      i_b_z    0     0     0  0],'modified');   

%right wing skeleton
L1=link([pi/2   0.035  Q(1,2)     0    0        m_s      0        0     0       i_s_x    i_s_y      i_s_z    0     0     0  0],'modified');  
L2=link([-pi/2   l1    Q(1,3)     0    0        m1      l1/2      0     0         0       0          i1      0     0     0  0],'modified');  
L3=link([0       l2    Q(1,4)     0    0        m2      l2/2      0     0         0       0          i2      0     0     0  0],'modified');  
L4=link([0       Ltip  Q(1,5)     0    0       m_dig   Ltip/2     0     0         0       0          i3      0     0     0  0],'modified');  


BAT_corke=robot({L0 L1 L2 L3 L4}); 
BAT_corke.name='BAT_corke';
n=BAT_corke.n;
Q1 = [Q(1,1) Q(1,2) Q(1,3) Q(1,4) Q(1,5)];
%Plotting 1 wing-beat cycle for downstroke and upstroke 
   plot(BAT_corke,Q,'joints');
%   drivebot(BAT_corke,Q1);
% 
%%Time Vector
step_2 = (1/freq)/(fil-1);
Ti=0:step_2:(1/freq);
GRAV = [0 0 0 0 0 -g]';
%F_load = [0 0 0 0 0 -0.001]';  due to aerodynamics load produced upon the wing's membrane
F_load = [0 0 0 0 0 0]';

Links = BAT_corke.link;

for i=1:n
    DHC(i,:) = Links{i}.dh;
end

%3D Cartesian Wingtip trajectory Reconstruction based on Forward kinematics
for i=1:fil
    rot=fkine2FB(DHC,Q(i,:));
    p(i,:) = rot(1:3,4)'; 
end
%plot3(p(:,1),p(:,2),p(:,3))

%Dynamics Simulations (Peter corke toolbox fixed-base)
%**************************************************************************
%Computing Forces to generate motion within the Follow-The-Leader assigment
%n: number of robots within the MRS.
%DHC:   alpha  a teta  d  sigma  m  Ixx  Iyy  Izz  sx  sy  sz
%Joint Trajectory:  Q,dQ,d2Q  of nxm, where n=# of trajectory points and m=#degrees of freedom   
%6-dimensional External Force: Fext
%6-dimensional gravity vector: GRAV

%[Torques,K] = inv_dyn_JDC(n,DHC,Q,dQ,d2Q,Fext,GRAV);
 Torques_corke = rne(BAT_corke,Q(:,1:5),dQ(:,1:5),d2Q(:,1:5));
%**************************************************************************

%**************************************************************************
%Dynamics Simulations (Roy Featherstone Fixed-base (toolbox) 1-WING
%**************************************************************************

BAT_Feat.NB = n;
BAT_Feat.parent = [0:n-1];
BAT_Feat.pitch = zeros(1,n);

for i = 1:n
    DH_row = Links{i}.dh;
    BAT_Feat.Xtree{i} = Xtrans([0,0,DH_row(4)]) * Xrotx(DH_row(1)) * Xtrans([DH_row(2),0,0]);
end

BAT_Feat.I{1} = mcI( m_b,    [0,      0,     0], diag([i_b_x,    i_b_y,      i_b_z]) );
BAT_Feat.I{2} = mcI( m_s,    [0,      0,     0], diag([i_s_x,    i_s_y,      i_s_z]) );
BAT_Feat.I{3} = mcI( m1,     [l1/2,   0,     0], diag([0,      0,          i1]) );
BAT_Feat.I{4} = mcI( m2,     [l2/2,   0,     0], diag([0,      0,          i2]) );
BAT_Feat.I{5} = mcI( m_dig,  [Ltip/2, 0,     0], diag([0,      0,          i3]) );

%Computing forces of the fixed-base system
for i = 1:fil
    Torques_feat(i,:) = ID( BAT_Feat, Q(i,1:5), dQ(i,1:5), d2Q(i,1:5) );
end


%**************************************************************************
%Dynamics Simulations (Roy Featherstone Floating-base (toolbox) 1-WING
%**************************************************************************

%transforming fixed-based bat model to floating base 
fb_BAT_Feat = floatbase( BAT_Feat );

for i = 1:fil    
    %adding 6-dimensional virtual base beetween the fixed base and the
    %floating base of the robot
    Q_f = [0 0 0 0 0 0 Q(i,1) Q(i,2) Q(i,3) Q(i,4) Q(i,5)]';
    dQ_f = [0 0 0 0 0 0 dQ(i,1) dQ(i,2) dQ(i,3) dQ(i,4) dQ(i,5)]';
    d2Q_f = [0 0 0 0 0 0 d2Q(i,1) d2Q(i,2) d2Q(i,3) d2Q(i,4) d2Q(i,5)]';
    
    %Computing floating base forward dynamics to set the 6D position,
    %velocity and acceleration of the floating base
    [X,v,a] = fbKin( Q_f, dQ_f, d2Q_f );
    
    %Computing floating base inverse dynamics to find the joint forces and
    %the acceleration of the floating base produced by those forces 
    [fbf,afb1,tau] = IDf( fb_BAT_Feat, X, v, Q(i,:), dQ(i,:), d2Q(i,:) );       
    
    Torques_feat_f(i,:) = tau;
    base_accel(:,i) = afb1;
    base_force(:,i) = fbf;
end
%**************************************************************************



 
 